#ifndef __POSE_ESTIMATOR_HPP__
#define __POSE_ESTIMATOR_HPP__

#include <stdio.h>
#include <iostream>
#include <fstream>
#include <string>

#include <opencv/cv.h>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <vector>
//#include "OpenCV/matwork.hpp"
#include "MatWork.hpp"
#include "ZhangPoseEstimator.hpp"


using namespace cv;
using namespace std;


class PoseEstimator
{
public:

	struct CamIntrinsics
	{
		double fx, fy, cx, cy;
	};

	PoseEstimator();

	~PoseEstimator();

	int Estimate(const Mat H, Mat& M);

	
private:

	int frame_num;
	int fcount;
	Mat B;
	CamIntrinsics cam;
	vector<Mat> Bv;

	int Solve();

	int Calc_vij(const Mat hi, const Mat hj, Mat& vij);

};



#endif